%% Saturn
warning off
clc;clear;
format longg
options_negx = odeset('RelTol',1e-13,'AbsTol',1e-13,'Events',@NegXcrossing);
options=odeset('RelTol',1e-14,'AbsTol',1e-22);
options_fmincon = optimoptions('fmincon', 'MaxFunctionEvaluations', 5000, ...
    'MaxIterations', 5000,'ConstraintTolerance',1e-6,'Algorithm','Interior-point','stepTolerance',1e-21);
%% Inputs.
G           = 6.674e-11 * ((1/1000)^3); % Gravitational parameters

mass_central = 1.989e30;
%[] Mass of central planet

mass_moon = 5.972e24;         %Callisto
%[] mass of moons

DU = 151.73e6;           %Callisto
%[km] Semi-major axis of Moons, used as distance units for each CRTBP
%environment.

moonName = {'Luna'};
%[] Name of the Moons

thetao = 0;
%[] Initial position of the moon relative to the first point of aries

GM_central = mass_central*G;
%[] Gravitational parameters

GM_moon = mass_moon*G;
%[] Gravitational parameters

N = length(mass_moon);
%[] Number of Moons

u = zeros(N,1);
for ii = 1:N
    u(ii) = GM_moon(ii)/(GM_moon(ii)+GM_central);
end
%[] Gravataional ratio

TU = zeros(N,1);
VU = zeros(N,1);
for ii = 1:N
    TU(ii) = sqrt(DU(ii)^3/GM_central);
    VU(ii) = DU(ii)/TU(ii);
end
%[] Time constant for each crtbp system

theta_dot = zeros(1,N);
for ii = 1:length(theta_dot)
    theta_dot(ii) = sqrt(GM_central*DU(ii))/DU(ii)^2;
end
%[] Theta dot for each planet.


planetNumb = 1;
[L1,L2,L3,L4,L5] = librationPoints(u(planetNumb));
L_ = [L1,L2,L3,L4,L5];
for ii = 1:length(L_)
    L_pos = L_(:,ii);
    J_L(ii) = jacobiConst(L_pos,zeros(3,1),u(planetNumb));
end





%%
siteName = {'KMTNet-CTIO','KMTNet-SSO','KMTNet-SAAO'};
Re = 6378;
alt = [2.167;1.143;1.762];
lambda_dms = [-70,48,14.4;...
              149,03,45.2;...
              20,48,37.6];

phi_dms = -[30,10,02.0;...
           31,16,16.3;...
           32,22,43.9];

dt = 0.00001;
%%%
days = 3;
month = 11;
%%%
Tvec = 0:dt:2*pi*(days/365);

% define L4 in SCI
L4sci = zeros(6,length(Tvec));
for ii = 1:length(Tvec);
    
    L4sci(:,ii) = C2I_primary([L4;0;0;0],u,DU,VU,Tvec(ii));

end

% Define Earth in SCI
Earthsci = zeros(6,length(Tvec));
for ii = 1:length(Tvec);
    
    Earthsci(:,ii) = C2I_primary([1;0;0;0;0;0],u,DU,VU,Tvec(ii));

end


% get Rsci_L4/e

for ii = 1:length(Tvec)
    Rsci_L4e(:,ii) = L4sci(:,ii) - Earthsci(:,ii);
end

% get Reci_L4/e
eci2sci = ECI2SCI(2035);
sci2eci = eci2sci';
Reci_L4e=zeros(3,length(Tvec));
for ii = 1:length(Tvec)
    Reci_L4e(:,ii) = sci2eci*Rsci_L4e(1:3,ii);
end
figure;
plot3(Reci_L4e(1,:),Reci_L4e(2,:),Reci_L4e(3,:))
% transform Reci_L4/e to Recef_L4/e
UTC = [2035,month,1,0,0,0];
JD = JulianDate(UTC);
for ii = 1:length(Tvec);
    JD_temp(ii) = JD+(Tvec(ii)*TU)/3600/24;
    UTC_list(ii,:) = CalendarDate(JD_temp(ii));
end
Recef_L4e = zeros(3,length(Tvec));
for ii = 1:length(Tvec)
    Teci2ecef = Eci2Ecef(UTC_list(ii,:));
    Recef_L4e(:,ii) = Teci2ecef*Reci_L4e(:,ii);
end


% Transform Recef_L4/e to Rsez_L4/e and Get Rsez_gs/e

for ii = 1:3
    Tecef2sez = Ecef2Sez(Dms2Deg(phi_dms(ii,:))*pi/180,Dms2Deg(lambda_dms(ii,:))*pi/180)
    Rsez_gse = [Re+alt(ii)]*[0;0;1];
    for jj = 1:length(Tvec)
        Rsez_L4e{ii}(:,jj) = Tecef2sez*Recef_L4e(:,jj);
        Rsez_L4gs{ii}(:,jj) = Rsez_L4e{ii}(:,jj)-Rsez_gse;
        Azimuth{ii}(jj) = atan2(Rsez_L4gs{ii}(2,jj),Rsez_L4gs{ii}(1,jj)) * 180 / pi;
        Elevation{ii}(jj) = asin(Rsez_L4gs{ii}(3,jj) / norm(Rsez_L4gs{ii}(:,jj))) * 180 / pi;
        if Elevation{ii}(jj)<0
            Azimuth{ii}(jj) = nan;
            Elevation{ii}(jj) = nan;
        end
    end
end




figure; 
subplot(2,1,1); hold on;
for ii = 1:3
    plot(JD_temp,Azimuth{ii},'LineWidth',2)
end
xlabel('Julian Date');
ylabel('Azimuth [deg]');
titlestring = sprintf('Month = %d',month)
title(titlestring);
legend(siteName)
set(gca,'FontSize',16);
subplot(2,1,2); hold on;
for ii = 1:3
    plot(JD_temp,Elevation{ii},'LineWidth',2)
end
xlabel('Julian Date');
ylabel('Elevation [deg]');
set(gca,'FontSize',16);








